//
// Created by zhengjuping on 24-7-10.
//

#include <driver/i2c.h>
#include <esp_log.h>
#include <cstring>
#include "include/BMP280.h"

#define I2C_MASTER_NUM              0

#define ACK_CHECK_EN                0x1                        /*!< I2C master will check ack from slave*/
#define ACK_VAL                     0x0                             /*!< I2C ack value */
#define NACK_VAL                    0x1                            /*!< I2C nack value */

#define BMP280_I2C_ADDR              0x76

#define TAG  "BMP280"

BMP280::BMP280() : mTfine(0){
    memset(&mBmp280Calib, 0, sizeof(mBmp280Calib));
    setSampling(BMP280_I2C_ADDR);
    readCoefficients(BMP280_I2C_ADDR);
}

BMP280::~BMP280() {

}

int32_t BMP280::getValue(float &pressure, float &temperature) {
    int32_t ret = -1;
    readTemperatureData(BMP280_I2C_ADDR, temperature);
    ret = readPressureData(BMP280_I2C_ADDR, pressure);
    return ret;
}

int32_t BMP280::read16(uint8_t addr, uint8_t reg, uint16_t &data) {
    uint8_t buffer[2]{};
    i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create();
    i2c_master_start(cmdHandle);
    i2c_master_write_byte( cmdHandle, (addr << 1) | I2C_MASTER_WRITE, ACK_CHECK_EN );
    i2c_master_write_byte( cmdHandle, reg, ACK_CHECK_EN );
    i2c_master_stop(cmdHandle);
    esp_err_t ret = i2c_master_cmd_begin((i2c_port_t)I2C_MASTER_NUM, cmdHandle, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmdHandle);

    vTaskDelay(100 / portTICK_PERIOD_MS);

    cmdHandle = i2c_cmd_link_create();
    i2c_master_start(cmdHandle);
    i2c_master_write_byte( cmdHandle, (addr << 1) | I2C_MASTER_READ, ACK_CHECK_EN );
    i2c_master_read(cmdHandle, buffer, 2, I2C_MASTER_LAST_NACK);
    i2c_master_stop(cmdHandle);
    ret = i2c_master_cmd_begin((i2c_port_t)I2C_MASTER_NUM, cmdHandle, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmdHandle);

    data = (uint16_t)(buffer[0] << 8) | (uint16_t)buffer[1];
    return 0;
}

int32_t BMP280::read16_LE(uint8_t addr, uint8_t reg, uint16_t &data) {
    uint16_t temp = 0;
    read16(addr, reg, temp);
    data =  (temp >> 8) | (temp << 8);
    return 0;
}

int32_t BMP280::readS16(uint8_t addr, uint8_t reg, int16_t &data) {
    uint16_t tmp = 0;
    read16(addr, reg, tmp);
    data = (int16_t )tmp;
    return 0;
}

int32_t BMP280::readS16_LE(uint8_t addr, uint8_t reg, int16_t &data) {
    uint16_t tmp = 0;
    read16_LE(addr, reg, tmp);
    data = (int16_t )tmp;
    return 0;
}

int32_t BMP280::read24(uint8_t addr, uint8_t reg, uint32_t &data) {
    uint8_t buffer[3]{0,0,0};
    i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create();
    i2c_master_start(cmdHandle);
    i2c_master_write_byte( cmdHandle, (addr << 1) | I2C_MASTER_WRITE, ACK_CHECK_EN );
    i2c_master_write_byte( cmdHandle, reg, ACK_CHECK_EN );
    i2c_master_stop(cmdHandle);
    esp_err_t ret = i2c_master_cmd_begin((i2c_port_t)I2C_MASTER_NUM, cmdHandle, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmdHandle);

    vTaskDelay(200 / portTICK_PERIOD_MS);

    cmdHandle = i2c_cmd_link_create();
    i2c_master_start(cmdHandle);
    i2c_master_write_byte( cmdHandle, (addr << 1) | I2C_MASTER_READ, ACK_CHECK_EN );
    i2c_master_read(cmdHandle, buffer, 3, I2C_MASTER_LAST_NACK);
    i2c_master_stop(cmdHandle);
    ret = i2c_master_cmd_begin((i2c_port_t)I2C_MASTER_NUM, cmdHandle, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmdHandle);

    data = uint32_t (buffer[0]) << 16 | uint32_t (buffer[1]) << 8 |
            uint32_t(buffer[2]);
    return ret;
}

int32_t BMP280::write8(uint8_t addr, uint8_t reg, uint8_t data) {
    i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create();
    i2c_master_start(cmdHandle);
    i2c_master_write_byte( cmdHandle, (addr << 1) | I2C_MASTER_WRITE, ACK_CHECK_EN );
    i2c_master_write_byte( cmdHandle, reg, ACK_CHECK_EN );
    i2c_master_write_byte( cmdHandle, data, ACK_CHECK_EN );
    i2c_master_stop(cmdHandle);
    esp_err_t ret = i2c_master_cmd_begin((i2c_port_t)I2C_MASTER_NUM, cmdHandle, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmdHandle);
    return 0;
}

void BMP280::setSampling(uint8_t addr) {
    write8( addr,BMP280_REGISTER_CONFIG, 0x00 );
    write8( addr,BMP280_REGISTER_CONTROL, 0x3F );
}

void BMP280::readCoefficients(uint8_t addr) {
    read16_LE(addr,  BMP280_REGISTER_DIG_T1, mBmp280Calib.dig_T1);
    readS16_LE(addr, BMP280_REGISTER_DIG_T2, mBmp280Calib.dig_T2);
    readS16_LE(addr, BMP280_REGISTER_DIG_T3, mBmp280Calib.dig_T3);

    read16_LE(addr,  BMP280_REGISTER_DIG_P1, mBmp280Calib.dig_P1);
    readS16_LE(addr, BMP280_REGISTER_DIG_P2, mBmp280Calib.dig_P2);
    readS16_LE(addr, BMP280_REGISTER_DIG_P3, mBmp280Calib.dig_P3);
    readS16_LE(addr, BMP280_REGISTER_DIG_P4, mBmp280Calib.dig_P4);
    readS16_LE(addr, BMP280_REGISTER_DIG_P5, mBmp280Calib.dig_P5);
    readS16_LE(addr, BMP280_REGISTER_DIG_P6, mBmp280Calib.dig_P6);
    readS16_LE(addr, BMP280_REGISTER_DIG_P7, mBmp280Calib.dig_P7);
    readS16_LE(addr, BMP280_REGISTER_DIG_P8, mBmp280Calib.dig_P8);
    readS16_LE(addr, BMP280_REGISTER_DIG_P9, mBmp280Calib.dig_P9);
}

int32_t BMP280::readTemperatureData(uint8_t addr, float &Temperature) {
    uint32_t var1 = 0, var2 = 0;
    uint32_t adc_T = 0;
    read24(BMP280_I2C_ADDR, BMP280_REGISTER_TEMPDATA, adc_T);
    adc_T >>= 4;

    var1 = ((((adc_T >> 3) - ((int32_t)mBmp280Calib.dig_T1 << 1))) *((int32_t)mBmp280Calib.dig_T2)) >> 11;

    var2 = (((((adc_T >> 4) - ((int32_t)mBmp280Calib.dig_T1)) * ((adc_T >> 4) - ((int32_t)mBmp280Calib.dig_T1))) >>12) *
            ((int32_t)mBmp280Calib.dig_T3)) >>14;

    mTfine = var1 + var2;
    auto T = float((mTfine * 5 + 128) >> 8);
    Temperature   = T / 100;
    return 0;
}

int32_t BMP280::readPressureData(uint8_t addr, float &pressure) {
    int64_t var1, var2, p;

    // Must be done first to get the t_fine variable set up
    uint32_t adc_P = 0;
    read24(addr, BMP280_REGISTER_PRESSUREDATA, adc_P);
    adc_P >>= 4;

    var1 = ((int64_t)mTfine) - 128000;
    var2 = var1 * var1 * (int64_t)mBmp280Calib.dig_P6;
    var2 = var2 + ((var1 * (int64_t)mBmp280Calib.dig_P5) << 17);
    var2 = var2 + (((int64_t)mBmp280Calib.dig_P4) << 35);
    var1 = ((var1 * var1 * (int64_t)mBmp280Calib.dig_P3) >> 8) +
           ((var1 * (int64_t)mBmp280Calib.dig_P2) << 12);
    var1 = (((((int64_t)1) << 47) + var1)) * ((int64_t)mBmp280Calib.dig_P1) >> 33;

    if (var1 == 0) {
        return -1;  // avoid exception caused by division by zero
    }

    p    = 1048576 - adc_P;
    p    = (((p << 31) - var2) * 3125) / var1;
    var1 = (((int64_t)mBmp280Calib.dig_P9) * (p >> 13) * (p >> 13)) >> 25;
    var2 = (((int64_t)mBmp280Calib.dig_P8) * p) >> 19;

    p = ((p + var1 + var2) >> 8) + (((int64_t)mBmp280Calib.dig_P7) << 4);
    pressure = p / 256;
    return 0;
}


